//!!!!! ROLLTILT parameter now included
//!!!!! fast return to manual only implimented for Auto => Manual transition
//!!!!! at some point need to do it for the 2 navigation modes (Aile/Yaw) => Manual transition
//!!!!! Boost terms still need to be zeroed in servoMix.c
//!!!!! DO NOT USE NAVIGATION MODES UNTIL THIS IS DONE...WILL NOT WORK

#include "p30f4011.h"
#include "definesRmat.h"
#include "defines.h"

//!!!!! ROLLTILT parameter to compensate for tail rotor thrust
//!!!!! factor of 2.0 calibrates servo throw to real angle
//!!!!! may not be universally true for all swash setups
#define ROLLTILTADJUST ((int)((2.0*RMAX*ROLLTILT)/57.3))

int yawkpail = YAWKP_AILERON*RMAX ;
int yawkdail = YAWKD_AILERON*SCALEGYRO*RMAX ;

int rollkp = ROLLKP*RMAX ;
int rollkd = ROLLKD*SCALEGYRO*RMAX ;


void rollCntrl(void)
{
	union longww rollAccum ;
	union longww dotprod ;
	union longww crossprod ;
	union longww gyroRollFeedback ;
	union longww gyroYawFeedback ;
	int desiredX ;
	int desiredY ;
	int actualX ;
	int actualY ;
//!!!!! add roll variable for fast return to manual
	union longww roll ;

#ifdef TestGains
	flags._.GPS_steering = 1 ;
#endif 

//!!!!! navigation mode code
//!!!!! Rudder navigation done in yawCntrl
	if ( AILERON_NAVIGATION && flags._.GPS_steering )
	{
#ifdef TestGains
		desiredX = -cosine ( 64 ) ;
		desiredY = sine ( 64 ) ;
#else
		desiredX = -cosine( desired_dir ) ;
		desiredY = sine( desired_dir ) ;
#endif
		actualX = rmat[1] ;
		actualY = rmat[4] ;
		dotprod.WW = __builtin_mulss( actualX , desiredX ) + __builtin_mulss( actualY , desiredY ) ;
		crossprod.WW = __builtin_mulss( actualX , desiredY ) - __builtin_mulss( actualY , desiredX ) ;
		crossprod.WW = crossprod.WW<<2 ;
		if ( dotprod._.W1 > 0 )
		{
			rollAccum.WW = __builtin_mulss( crossprod._.W1 , yawkpail ) ;
		}
		else
		{
			if ( crossprod._.W1 > 0 )
			{
				rollAccum._.W1 = yawkpail/4 ;
			}
			else
			{
				rollAccum._.W1 = -yawkpail/4 ;
			}
		}
	}
	else
	{
		rollAccum.WW = 0 ;
	}
#ifdef TestGains
	flags._.pitch_feedback = 1 ;
#endif


//!!!!! This is where we check if we were in Auto but just switched to Manual	
	if ( ROLL_STABILIZATION && flags._.pitch_feedback && (pwIn[MODE_SWITCH_INPUT_CHANNEL] > MODE_SWITCH_THRESHOLD_LOW))
	{
		gyroRollFeedback.WW = __builtin_mulss( rollkd , omegaAccum[1] ) ;
//!!!!! roll tilt offset included
		rollAccum.WW += __builtin_mulss( rmat[6]+ ROLLTILTADJUST , rollkp ) ;
//!!!!! also take snapshot of Accum for final calculation
		roll.WW = rollAccum.WW ;
	}
	else
	{
		gyroRollFeedback.WW = 0 ;
//!!!!! add additional code for fast return to manual here
//!!!!! note that we must take care of boost terms in servoMix.c
		roll.WW = 0 ;
	}
//!!!!! do we want yaw stabilization for roll with helis????	
	if ( YAW_STABILIZATION_AILERON && (flags._.GPS_steering || flags._.pitch_feedback) )
	{
		gyroYawFeedback.WW = __builtin_mulss( yawkdail , omegaAccum[2] ) ;
	}
	else
	{
		gyroYawFeedback.WW = 0 ;
	}
//!!!!! changed to use roll._.W1 rather than rollAccum._.W1 for fast return to manual	
	roll_control = (long)roll._.W1 - (long)gyroRollFeedback._.W1 - (long)gyroYawFeedback._.W1 ;
	// Servo reversing is handled in servoMix.c

	return ;
}
